package Analyser;

import java.util.ArrayList;
import java.util.HashMap;

import maze.Direction;
import pacman.Ghost;

public class SectorAnalyserV2 implements SectorAnalyser {

	public int readingsPerQuadrant = 1;
	public int angleMargin = 20;

	@Override
	public HashMap<Direction, DistanceToWall> ExploreSector(Ghost ghost) {
			
		HashMap<Direction, DistanceToWall> ret = new HashMap<Direction, DistanceToWall>();
		
		int sectorSize = 40;
		
		int margin = 5;
		int increment = angleMargin*2 / (readingsPerQuadrant-1);
		
		ArrayList<Integer> front = new ArrayList<Integer>();
		ArrayList<Integer> left = new ArrayList<Integer>();
		ArrayList<Integer> back = new ArrayList<Integer>();
		ArrayList<Integer> right = new ArrayList<Integer>();
		
		int angle = 0;
		for(int i=0; i<360; i+=90)
		{
			angle = i - angleMargin;
			
			for(int j=0; j<readingsPerQuadrant; j++)
			{		
				if(j!=0)
					angle += increment;
				//angle += j * angleMargin / (readingsPerQuadrant*0.5f);
				rotateToAngle(angle, ghost);
				
				int expected = 0;
				int reading = 0;
				
				
				
				if(i<45 || i>360-45)
				{
					expected = (int)(sectorSize*0.5/Math.cos(angle/180*Math.PI)+0.5f);
					reading = ghost.getUltrasonicSensor().getDistance();
					front.add(expected-reading);
				}
				else if(i<90+45)
				{
					expected = (int)(sectorSize*0.5/Math.cos((90-angle)/180*Math.PI)+0.5f);
					reading = ghost.getUltrasonicSensor().getDistance();
					left.add(expected-reading);
				}
				else if(i<180+45)
				{
					expected = (int)(sectorSize*0.5/Math.cos((180-angle)/180*Math.PI)+0.5f);
					reading = ghost.getUltrasonicSensor().getDistance();
					back.add(expected-reading);
				}
				else
				{
					expected = (int)(sectorSize*0.5/Math.cos((270-angle)/180*Math.PI)+0.5f);
					reading = ghost.getUltrasonicSensor().getDistance();
					right.add(expected-reading);
				}
				
				
			}
		}
		
		//gemiddelde error per quadrant berekenen
		int[] errors = new int[4];
		errors[0] = getMeanValue(front);
		errors[1] = getMeanValue(left);
		errors[2] = getMeanValue(back);
		errors[3] = getMeanValue(right);
		
		
		int[] distances = new int[4];
		distances[0] = sectorSize/2 - errors[0];
		distances[1] = sectorSize/2 - errors[1];
		distances[2] = sectorSize/2 - errors[2];
		distances[3] = sectorSize/2 - errors[3];
		
		//bepalen of er een muur staat
		int[] walls = new int[4];		
		for(int i=0; i<4; i++)
		{
			if (Math.abs(errors[i]) < margin) {
				walls[i] = 1;
			}				
			else {
				walls[i] = 2;
			}
		}
		
		//lijst opstellen met als eerste direction die van de robot, dan ccw rest directions
		Direction[] directions = new Direction[4];
		directions[0] = Direction.SOUTH;
		directions[1] = Direction.EAST;
		directions[2] = Direction.NORTH;
		directions[3] = Direction.WEST;
		int direction = generateDirection(ghost.getRotation());
		if (direction == 3) { //Kijkt nr zuiden
			shiftArray(directions, 2);
		} else if (direction == 2) { //Kijkt naar westen
			shiftArray(directions, 1);
		} else if (direction == 4) { //Kijkt naar oosten
			shiftArray(directions, 3);
		}
		
		for(int i=0; i<4; i++)
		{
			ret.put(directions[i], new DistanceToWall(walls[i], distances[i]));
		}
		ghost.resetSensorMotor();
		
		return ret;		
	}

	private void rotateToAngle(int angle, Ghost g) {
		int currentAngle = g.getSensorRotation();
		
		g.rotateSensorMotor(angle - currentAngle%360);
		
	}

	private int getMeanValue(ArrayList<Integer> front) {
		
		int total = 0;
		int count = 0;
		
		for(int i=0; i<front.size();i++)
		{
			if(front.get(i) != 255)
			{
				total += front.get(i);
				count ++;
			}			
		}
		
		if(count == 0)
			return 255;
		
		return (int)(total/count + 0.5);
	}
	
	/**
	 * Returns the integer value of this Ghost's direction.
	 * "Hierbij wordt de waarde 
	 * 1 gebruikt voor noord naar zuid,  == ZUID
	 * 2 voor oost naar west, 			 == WEST
	 * 3 voor zuid naar noord, 			 == NOORD
	 * 4 voor west naar oost."			 == OOST
	 */
	public int generateDirection(int rotation) {
		int rot = rotation % 360;
		rot = rot < 0 ? (360 - Math.abs(rot)) : rot;
		int direction = -1;
		if (rot > 45 && rot < 135) {
			direction = 3;
		} else if (rot >= 135 && rot < 225) {
			direction = 2;
		} else if (rot >= 225 && rot < 315) {
			direction = 1;
		} else {
			direction = 4;
		}
		return direction;
	}

	public Direction[] shiftArray(Direction[] directions, int amount){
		for (int j = 0; j < amount; j++) {
			Direction a = directions[directions.length - 1];
            int i;
            for (i = directions.length - 1; i > 0; i--)
            	directions[i] = directions[i - 1];
            directions[i] = a;
		}
		return directions;
	}
}
